#include "Structural.h"
#include "Node.h"

Structural::Structural(float rest, float stiff, float damp, void* A, void* B) {
	R = rest;
	kH = stiff;
	kD = damp;
	nodeA = A;
	nodeB = B;
}

Structural::~Structural() {

}

Vector3f Structural::getForce() {
	Vector3f result;
	result = HookForce() + DampingForce();
	return result;
}

Vector3f Structural::HookForce() {
	Vector3f hook;
	Vector3f L;
	float xL;

	Node *A = (Node*)nodeA;
	Node *B = (Node*)nodeB;

	L = B->getPosition() - A->getPosition();
	xL = L.mag();

	hook = (L / xL) * (-kH) * (xL - R);

	return hook;
}

Vector3f Structural::DampingForce() {
	Vector3f damping;
	Vector3f vA;
	Vector3f vB;
	Vector3f L;
	float xL;

	Node *A = (Node*)nodeA;
	Node *B = (Node*)nodeB;

	L = B->getPosition() - A->getPosition();
	xL = L.mag();
	vA = A->getVelocity();
	vB = B->getVelocity();

	damping = (L / xL) * (((vA - vB) * L) / xL) *(-kD);

	return damping;
}